from launch import LaunchDescription
from launch_ros.actions import Node
from launch.substitutions import LaunchConfiguration
from launch.actions import ExecuteProcess, IncludeLaunchDescription, DeclareLaunchArgument
from launch.launch_description_sources import PythonLaunchDescriptionSource
from ament_index_python.packages import get_package_share_directory
import os,xacro


def generate_launch_description():
    # Gazebo launch
    # Start Gazebo server

    world_file = os.path.join(
        get_package_share_directory("path_follower"),
        "worlds",
        "room.world",
    )

    start_gazebo_cmd =  ExecuteProcess(
        cmd=['gazebo', '--verbose','-s', 'libgazebo_ros_init.so', '-s', 'libgazebo_ros_factory.so', world_file],
        output='screen')

    xacro_file = os.path.join(
        get_package_share_directory("path_follower"),
        "urdf",
        "slim.xacro",
    )
    doc = xacro.parse(open(xacro_file))
    xacro.process_doc(doc)

    print("xacro_file : {}".format(xacro_file))

    # 定义robot_state_publisher节点
    robot_state_publisher_node = Node(
        package='robot_state_publisher',
        executable='robot_state_publisher',
        name='robot_state_publisher',
        output="screen",
        parameters=[{'robot_description': doc.toxml()}]
    )

    # 在 Gazebo 中加载机器人模型
    spawn_robot_cmd = Node(
        package="gazebo_ros",
        executable="spawn_entity.py",
        arguments=[
            '-entity', 'slim',   
            '-topic', 'robot_description', 
        ],
        output="screen",
    )

    load_joint_state_controller = ExecuteProcess(
        cmd=['ros2', 'control', 'load_controller', '--set-state', 'start', 'joint_state_broadcaster'],
        output='screen'
    )

    load_joint_forward_velocity_controller = ExecuteProcess(
        cmd=['ros2', 'control', 'load_controller', '--set-state', 'start', 'forward_velocity_controller'],
        output='screen'
    )

    load_joint_trajectory_controller = ExecuteProcess(
        cmd=['ros2', 'control', 'load_controller', '--set-state', 'start', 'joint_trajectory_controller'],
        output='screen'
    )

    load_slim_contrller = Node(
        package='path_follower',
        executable='path_follower',
        name='path_follower',
        output='screen',
        parameters=[{'use_sim_time': True}]
    )

    # 启动建图
    slam_params_file = os.path.join(get_package_share_directory("path_follower"), "config", "slam_params.yaml")
    start_slam_cmd = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(
            os.path.join(
                get_package_share_directory("slam_toolbox"),
                "launch",
                "online_sync_launch.py",
            )
        ),
        launch_arguments={"use_sim_time": "true",
                          "params_file" : slam_params_file}.items(),
    )

    # 启动导航
    # 启动navigation2
    start_nav2_cmd = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(
            os.path.join(get_package_share_directory("path_follower"), "launch", "nav2_bringup_launch.py")
        )
    )
    
    # 启动rviz
    start_rviz_cmd = ExecuteProcess(
        cmd=['rviz2', '-d', os.path.join(get_package_share_directory("path_follower"), "rviz", "rviz2.rviz")],
        output='screen'
    )

    # 定义 LaunchDescription
    ld = LaunchDescription()

    ld.add_action(start_gazebo_cmd)
    ld.add_action(spawn_robot_cmd)
    ld.add_action(robot_state_publisher_node)
    ld.add_action(load_joint_state_controller)
    ld.add_action(load_joint_forward_velocity_controller)
    ld.add_action(load_joint_trajectory_controller)
    ld.add_action(load_slim_contrller)
    ld.add_action(start_slam_cmd)
    ld.add_action(start_nav2_cmd)

    ld.add_action(start_rviz_cmd)

    return ld
